#lang racket


(require "lang.rkt"
         "list.rkt"
         "math.rkt")

(provide 
 xyz
 xy
 yz
 xz
 x
 y
 z
 xyz-x
 xyz-y
 xyz-z
 xyz-r
 
 cyl
 cyl-rho
 cyl-phi
 cyl-z
 
 pol
 pol-rho
 pol-phi
 
 sph
 sph-r
 sph-phi
 sph-th
 
 u0
 ux
 uy
 uz
 uxy
 uyz
 uxz
 uxyz
 
 (rename-out (coord? is-coord))
 is-u0
 
 eq-c
 add-c
 sub-c
 dot-c
 cross-c
 collinear-cross-c
 angle-c
 collinearity-c
 
 mult-c
 
 distance
 midcoord
 norm
 symmetric
 
 add-x-c
 add-y-c
 add-z-c
 add-xy-c
 add-yz-c
 add-xz-c
 add-xyz-c
 
 add-cyl-c
 add-pol-c
 add-sph-c
 
 list-of-coord
 vector-of-coord
 coord-of-vector)


(define (cartesian-x<-spherical r phi th)
  (* r (cos phi) (sin th)))

(define (cartesian-y<-spherical r phi th)
  (* r (sin phi) (sin th)))

(define (cartesian-z<-spherical r phi th)
  (* r (cos th)))

(define (cartesian<-spherical r phi th)
  (list
   (cartesian-x<-spherical r phi th)
   (cartesian-y<-spherical r phi th)
   (cartesian-z<-spherical r phi th)))


(define (cartesian-x<-cylindrical rho phi z)
  (* rho (cos phi)))

(define (cartesian-y<-cylindrical rho phi z)
  (* rho (sin phi)))

(define (cartesian-z<-cylindrical rho phi z)
  z)

(define (cartesian<-cylindrical rho phi z)
  (list
   (cartesian-x<-cylindrical rho phi z)
   (cartesian-y<-cylindrical rho phi z)
   (cartesian-z<-cylindrical rho phi z)))


(define (coord-write c port mode)
  (write-string "c" port)
  (write-string (format "~a" (list-of-coord c)) port))

(struct coord (x y z)
  #:property prop:custom-write coord-write)

(define (λcoord c (λc λid) (λx λid) (λy λid) (λz λid))
  (λc
   (λx (xyz-x c))
   (λy (xyz-y c))
   (λz (xyz-z c))))


(define (xyz x y z)
  (coord x y z))

(define xy
  (case-lambda
    ((x y)
     (xyz x y 0))
    ((c x y)
     (struct-copy coord c (x x) (y y)))))

(define yz
  (case-lambda
    ((y z)
     (xyz 0 y z))
    ((c y z)
     (struct-copy coord c (y y) (z z)))))

(define xz
  (case-lambda
    ((x z)
     (xyz x 0 z))
    ((c x z)
     (struct-copy coord c (x x) (z z)))))

(define x
  (case-lambda
    ((x)
     (xyz x 0 0))
    ((c x)
     (struct-copy coord c (x x)))))

(define y
  (case-lambda
    ((y)
     (xyz 0 y 0))
    ((c y)
     (struct-copy coord c (y y)))))

(define z
  (case-lambda
    ((z)
     (xyz 0 0 z))
    ((c z)
     (struct-copy coord c (z z)))))

(define xyz-x coord-x)
(define xyz-y coord-y)
(define xyz-z coord-z)

(define (xyz-r c)
  (sqrt (+ (^2 (xyz-x c)) (^2 (xyz-y c)) (^2 (xyz-z c)))))


(define (cyl rho phi z)
  (apply xyz (cartesian<-cylindrical rho phi z)))

(define (cyl-rho c)
  (sqrt (+ (^2 (xyz-x c)) (^2 (xyz-y c)))))

(define (cyl-phi c)
  (sph-phi c))

(define (cyl-z c)
  (xyz-z c))


(define (pol rho phi)
  (cyl rho phi 0))

(define (pol-rho c)
  (cyl-rho (struct-copy coord c (z 0))))

(define (pol-phi c)
  (cyl-phi (struct-copy coord c (z 0))))


(define (sph r phi th)
  (apply xyz (cartesian<-spherical r phi th)))

(define (sph-r c)
  (xyz-r c))

(define (sph-phi c)
  (if (= (xyz-x c) (xyz-y c) 0)
      0
      (atan (xyz-y c) (xyz-x c))))

(define (sph-th c)
  (if (= (sph-r c) 0)
      0
      (acos (/ (xyz-z c) (sph-r c)))))


(define u0 (xyz 0 0 0))

(define ux (xyz 1 0 0))
(define uy (xyz 0 1 0))
(define uz (xyz 0 0 1))

(define uxy (xy 1 1))
(define uyz (yz 1 1))
(define uxz (xz 1 1))

(define uxyz (xyz 1 1 1))


(define (is-u0 c)
  (and
   (coord? c)
   (= (xyz-x c) 0)
   (= (xyz-y c) 0)
   (= (xyz-z c) 0)))

(define (eq-c c1 c2 (threshold-d 0))
  (define (eq-p)
    (and
     (= (xyz-x c1) (xyz-x c2))
     (= (xyz-y c1) (xyz-y c2))
     (= (xyz-z c1) (xyz-z c2))))
  
  (if (= threshold-d 0)
      (eq-p)
      (or (eq-p) (<= (distance c1 c2) threshold-d))))

(define add-c
  (let ((add-coord
         (λ (c1 c2)
           (xyz
            (+ (xyz-x c1) (xyz-x c2))
            (+ (xyz-y c1) (xyz-y c2))
            (+ (xyz-z c1) (xyz-z c2))))))
    (case-lambda
      ((c1 c2)
       (add-coord c1 c2))
      ((c1 c2 . cs)
       (reduce add-coord (list* c1 c2 cs))))))

(define sub-c
  (let* ((sub-coord
          (λ (c1 c2)
            (xyz
             (- (xyz-x c1) (xyz-x c2))
             (- (xyz-y c1) (xyz-y c2))
             (- (xyz-z c1) (xyz-z c2)))))
         (sub-coord-fn
          (λ (c1 c2)
            (sub-coord c2 c1))))
    (case-lambda
      ((c1 c2)
       (sub-coord c1 c2))
      ((c1 c2 . cs)
       (foldl sub-coord-fn c1 (cons c2 cs))))))

(define (dot-c c1 c2)
  (+
   (* (xyz-x c1) (xyz-x c2))
   (* (xyz-y c1) (xyz-y c2))
   (* (xyz-z c1) (xyz-z c2))))

(define (cross-c c1 c2)
  (xyz
   (-
    (* (xyz-y c1) (xyz-z c2))
    (* (xyz-z c1) (xyz-y c2)))
   (-
    (* (xyz-z c1) (xyz-x c2))
    (* (xyz-x c1) (xyz-z c2)))
   (-
    (* (xyz-x c1) (xyz-y c2))
    (* (xyz-y c1) (xyz-x c2)))))

(define (collinear-cross-c c)
  (define (collinear-pxp-1)
    (xyz 0 (xyz-z c) (- (xyz-y c))))
  
  (define (collinear-pxp-2)
    (xyz (xyz-z c) 0 (- (xyz-x c))))
  
  (define (collinear-pxp-3)
    (xyz (xyz-y c) (- (xyz-x c)) 0))
  
  (let ((n (collinear-pxp-1)))
    (if (is-u0 n)
        (let ((n (collinear-pxp-2)))
          (if (is-u0 n)
              (collinear-pxp-3)
              n))
        n)))

(define (angle-c c1 c2 (normal #f))
  (define (aux)
    (acos
     (dot-c
      (mult-c c1 (/ (sph-r c1)))
      (mult-c c2 (/ (sph-r c2))))))
  
  (define (continuous-aux)
    (let ((p (cross-c c1 c2)))
      (if (or
           (eq-c u0 p)
           (> (dot-c normal p) 0))
          (aux)
          (- 2pi (aux)))))
  
  (if normal
      (continuous-aux)
      (aux)))

(define (collinearity-c c1 c2)
  (define (n//n n1 n2)
    (cond ((= n1 n2 0) #t)
          ((= n2 0) 0)
          (else (/ n1 n2))))
  
  (define (ns//ns)
    (filter
     (cλ (λ. not eqv?) true)
     (list
      (n//n (xyz-x c1) (xyz-x c2))
      (n//n (xyz-y c1) (xyz-y c2))
      (n//n (xyz-z c1) (xyz-z c2)))))
  
  (let ((ns (ns//ns)))
    (cond ((empty? ns) 0)
          ((apply = (cons (first ns) ns)) (first ns))
          (else 0))))


(define (mult-c c/r1 c/r2)
  (define (mult-coord c r)
    (xyz
     (* (xyz-x c) r)
     (* (xyz-y c) r)
     (* (xyz-z c) r)))
  
  (if (coord? c/r1)
      (mult-coord c/r1 c/r2)
      (mult-coord c/r2 c/r1)))


(define (distance c1 c2)
  (xyz-r (sub-c c1 c2)))

(define (midcoord c1 c2)
  (let ((axis (sub-c c2 c1)))
    (add-xyz-c c1 (/2 (xyz-x axis)) (/2 (xyz-y axis)) (/2 (xyz-z axis)))))

(define (norm c)
  (mult-c c (/ (sph-r c))))

(define (symmetric c)
  (λcoord c xyz - - -))


(define (add-x-c c r)
  (λcoord c xyz (cλ + r) λid λid))

(define (add-y-c c r)
  (λcoord c xyz λid (cλ + r) λid))

(define (add-z-c c r)
  (λcoord c xyz λid λid (cλ + r)))

(define (add-xy-c c x y)
  (λcoord c xyz (cλ + x) (cλ + y) λid))

(define (add-yz-c c y z)
  (λcoord c xyz λid (cλ + y) (cλ + z)))

(define (add-xz-c c x z)
  (λcoord c xyz (cλ + x) λid (cλ + z)))

(define (add-xyz-c c x y z)
  (λcoord c xyz (cλ + x) (cλ + y) (cλ + z)))

(define (add-cyl-c c rho phi z)
  (apply add-xyz-c c (cartesian<-cylindrical rho phi z)))

(define (add-pol-c c rho phi)
  (add-cyl-c c rho phi 0))

(define (add-sph-c c r phi th)
  (apply add-xyz-c c (cartesian<-spherical r phi th)))


(define (list-of-coord c)
  (λcoord c list))

(define (vector-of-coord c)
  (λcoord c vector))

(define (coord-of-vector v)
  (apply xyz (vector->list v)))